#include "dri_servo.h"
#include "dri_systick.h"

// 初始化TIM3用于舵机控制（使用多个通道）
void dri_servo_init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
    TIM_OCInitTypeDef TIM_OCInitStruct;
    
    // 使能GPIOA、GPIOB和TIM3时钟
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    
    // 配置PA6为复用功能(TIM3_CH1)
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(GPIOA, &GPIO_InitStruct);
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_TIM3);
    
    // 配置PA7为复用功能(TIM3_CH2)
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_7;
    GPIO_Init(GPIOA, &GPIO_InitStruct);
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_TIM3);
    
    // 配置PC8为复用功能(TIM3_CH3)
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8;
    GPIO_Init(GPIOC, &GPIO_InitStruct);
    GPIO_PinAFConfig(GPIOC, GPIO_PinSource8, GPIO_AF_TIM3);
    
    // 配置PC9为复用功能(TIM3_CH4)
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_9;
    GPIO_Init(GPIOC, &GPIO_InitStruct);
    GPIO_PinAFConfig(GPIOC, GPIO_PinSource9, GPIO_AF_TIM3);
    
    // 配置TIM3
    TIM_TimeBaseInitStruct.TIM_Prescaler = 84 - 1; // 84MHz/84 = 1MHz timer clock
    TIM_TimeBaseInitStruct.TIM_Period = 20000 - 1; // 20ms period (50Hz)
    TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStruct);
    
    // 配置PWM模式 - 通道1
    TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStruct.TIM_Pulse = 500; // 初始位置：1.5ms脉冲宽度
    
    TIM_OC1Init(TIM3, &TIM_OCInitStruct);
    TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
    
    // 配置PWM模式 - 通道2
    TIM_OCInitStruct.TIM_Pulse = 500;
    TIM_OC2Init(TIM3, &TIM_OCInitStruct);
    TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
    
    // 配置PWM模式 - 通道3
    TIM_OCInitStruct.TIM_Pulse = 500;
    TIM_OC3Init(TIM3, &TIM_OCInitStruct);
    TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
    
    // 配置PWM模式 - 通道4
    TIM_OCInitStruct.TIM_Pulse = 500;
    TIM_OC4Init(TIM3, &TIM_OCInitStruct);
    TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
    
    // 使能TIM3
    TIM_Cmd(TIM3, ENABLE);
}

// 设置舵机角度 - 通道1 (PA6)
void servo_set_angle_ch1(uint16_t angle)
{
    uint16_t pulse_width = 500 + (angle * 2000) / 180;
    TIM3->CCR1 = pulse_width;
}

// 设置舵机角度 - 通道2 (PA7)
void servo_set_angle_ch2(uint16_t angle)
{
    uint16_t pulse_width = 500 + (angle * 2000) / 180;
    TIM3->CCR2 = pulse_width;
}

// 设置舵机角度 - 通道3 (PC8)
void servo_set_angle_ch3(uint16_t angle)
{
    uint16_t pulse_width = 500 + (angle * 2000) / 180;
    TIM3->CCR3 = pulse_width;
}

// 设置舵机角度 - 通道4 (PC9)
void servo_set_angle_ch4(uint16_t angle)
{
    uint16_t pulse_width = 500 + (angle * 2000) / 180;
    TIM3->CCR4 = pulse_width;
}
void dog_stand(void)
{
    servo_set_angle_ch1(90);  // 左后腿中立
    servo_set_angle_ch2(90);  // 左前腿中立
    servo_set_angle_ch3(90);  // 右前腿中立
    servo_set_angle_ch4(90);  // 右后腿中立
    dri_systick_ms(600);      // 给舵机时间移动到位置
}

// 狗坐下姿势
void dog_sit(void)
{
    servo_set_angle_ch1(0);  // 左后腿弯曲
    servo_set_angle_ch2(90);  // 左前腿弯曲
    servo_set_angle_ch3(90); // 右前腿弯曲
    servo_set_angle_ch4(0); // 右后腿弯曲
    dri_systick_ms(600);      // 给舵机时间移动到位置
}

// 前进歩态
void dog_walk_forward(void)
{
   
     // 与前进相反的顺序
    servo_set_angle_ch1(135);   // 左后腿向前
    servo_set_angle_ch2(90);  // 左前腿向后
    servo_set_angle_ch3(45);   // 右前腿向前
    servo_set_angle_ch4(90);  // 右后腿向后
    dri_systick_ms(400);
    
    servo_set_angle_ch1(90);
    servo_set_angle_ch2(135);
    servo_set_angle_ch3(90);
    servo_set_angle_ch4(45);
    dri_systick_ms(400);
   
}

// 后退歩态
void dog_walk_backward(void)
{
   
     // 第一步：左前和右后腿向前，右前和左后腿向后
    servo_set_angle_ch1(45);  // 左后腿向后
    servo_set_angle_ch2(90);   // 左前腿向前
    servo_set_angle_ch3(135);  // 右前腿向后
    servo_set_angle_ch4(90);   // 右后腿向前
    dri_systick_ms(400);
    
    // 第二步：所有腿回到中立位置
    servo_set_angle_ch1(90);
    servo_set_angle_ch2(45);
    servo_set_angle_ch3(90);
    servo_set_angle_ch4(135);
    dri_systick_ms(400);
   
}


void dog_lie(void)
{
	servo_set_angle_ch1(0);  
    servo_set_angle_ch2(0);  
    servo_set_angle_ch3(0);  
    servo_set_angle_ch4(0);  
    dri_systick_ms(600);     
}

